/* Seng 486F Mechatronics - Mantis Cheng - Spring 2007

	Project 1
	January 2007 

	Brad Barclay
	Scott Craig
	Alexander Hoole

*/

#include "joystick.h"

#define NUM_SAMPLES		11
// File scope variables
static unsigned char sample[3][NUM_SAMPLES];
static unsigned char next_sample;
static int sum[3];

unsigned char normX[3] = {0, 128, 255};  
unsigned char normY[3] = {0, 128, 255};
static unsigned char raw_value[3][3][2] = { // Xpos, Ypos, X or Y
	{{0,0}, {0, 128}, {0, 255}}, 	// Xpos == LEFT, Ypos == BACK, MIDY, FORWARD
	{{128,0}, {128, 128}, {128, 255}}, 	// Xpos == MIDX, Ypos == BACK, MIDY, FORWARD
	{{255,0}, {255, 128}, {255, 255}} 	// Xpos == RIGHT, Ypos == BACK, MIDY, FORWARD
	};

static int correction_factors_set = 0;
static int leftY_corr[2];		// {deltaX, deltaY} from center position
static int rightY_corr[2];
static int backX_corr[2];
static int forwardX_corr[2];

static int min_half_range[2] = {128, 128}; // X, Y

// Forward declarations
static void single_sample(unsigned char SAMPN, int input_num);
static void set_correction_factors(void);

/**

*/
void joystick_init()
{
	int i = 0;
	
	/* Set up input pins */
	DIDR0 = ~PORTF_ENABLE_MASK; // Disable unused ADC input pins
	DDRF &= ~PORTF_ENABLE_MASK; // Set low for input
	PORTF &= ~PORTF_ENABLE_MASK;  // Write 0 to disable pull-up resistors
	
	// Initialise sample[][] and sum[]
	for(i = 0; i < NUM_SAMPLES; ++i)
	{
		single_sample(SAMP0, 0);
		single_sample(SAMP1, 1);
		single_sample(SAMP2, 2);
		++next_sample;
		if (next_sample >= NUM_SAMPLES) next_sample = 0;
	}

}


/**

*/
static 
void single_sample(unsigned char SAMPN, int input_num)
{
	ADMUX = SAMPN;
	ADCSRA = TAKE_SAMP;
	while (ADC_NOT_DONE()){}	

	sum[input_num] -= sample[input_num][next_sample];
	sample[input_num][next_sample] = ADCH;
	sum[input_num] += sample[input_num][next_sample];
}


/**

*/
void sample_joystick(unsigned char *X, unsigned char *Y, unsigned char *T)
{

	single_sample(SAMP0, 0);
	*X = (unsigned char) (sum[0] / NUM_SAMPLES);

	single_sample(SAMP1, 1);
	*Y = (unsigned char) (sum[1] / NUM_SAMPLES);

	single_sample(SAMP2, 2);
	*T = (unsigned char) (sum[2] / NUM_SAMPLES);

	++next_sample;
	if (next_sample >= NUM_SAMPLES) next_sample = 0;
}


/** Sets the minimum (maximum) values of X and Y to be the maximum
*   (minimum) of the values recorded in that position.
*
*	For example, if three values at the left side are 9, 11, and 12
*	then normX[LEFT] is set to 12.
*/
void calibrate_joystick(unsigned char Xpos, unsigned char Ypos)
{
	unsigned char Xval, Yval, Tval;
	int i;

	if (Xpos > 2 || Ypos > 2) 
		return; // fail silently
	
	for (i=0; i<NUM_SAMPLES; ++i)
	{
		sample_joystick(&Xval, &Yval, &Tval);
	}

	raw_value[Xpos][Ypos][0] = Xval;
	raw_value[Xpos][Ypos][1] = Yval;
}


/**
*
*/
void normalize_joystick(unsigned char *X, unsigned char *Y, unsigned char *T)
{
	int newX ,newY;
	newX = newY = 0;

	if (!correction_factors_set)
	{
		set_correction_factors();
	}


	// Correct skew
	if (*X <= raw_value[MIDX][MIDY][0])  
	{
		if (*Y <= raw_value[MIDX][MIDY][1])
		{
			// back left
			if (backX_corr[1] != 0)
				newX = *X - ((*Y - raw_value[MIDX][MIDY][1]) * backX_corr[0]) / backX_corr[1];
			if (leftY_corr[0] != 0)
				newY = *Y - ((*X - raw_value[MIDX][MIDY][0]) * leftY_corr[1]) / leftY_corr[0];
		}
		else 
		{
			// forward left
			if (forwardX_corr[1] != 0)
				newX = *X - ((*Y - raw_value[MIDX][MIDY][1]) * forwardX_corr[0]) / forwardX_corr[1];
			if (leftY_corr[0] != 0)
				newY = *Y - ((*X - raw_value[MIDX][MIDY][0]) * leftY_corr[1]) / leftY_corr[0];

		}
	}
	else 
	{
		if (*Y <= raw_value[MIDX][MIDY][1])
		{
			// back right
			if (backX_corr[1] != 0)
				newX = *X - ((*Y - raw_value[MIDX][MIDY][1]) * backX_corr[0]) / backX_corr[1];
			if (rightY_corr[0] != 0)
				newY = *Y - ((*X - raw_value[MIDX][MIDY][0]) * rightY_corr[1]) / rightY_corr[0];
		}

		else  
		{
			// forward right 
			if (forwardX_corr[1] != 0)
				newX = *X - ((*Y - raw_value[MIDX][MIDY][1]) * forwardX_corr[0]) / forwardX_corr[1];
			if (rightY_corr[0] != 0)
				newY = *Y - ((*X - raw_value[MIDX][MIDY][0]) * rightY_corr[1]) / rightY_corr[0];
		}
	}

	// Scale by minimum half range
	if (min_half_range[0] > 0)
		newX = (newX - 128) * 128 / min_half_range[0] + 128; 

	if (min_half_range[1] > 0)
		newY = (newY - 128) * 128 / min_half_range[1] + 128; 



	// Clip
	if (newX < 0) 			*X = 0;
	else if (newX > 255) 	*X = 255;
	else					*X = (unsigned char) newX;


	if (newY < 0) 			*Y = 0;
	else if (newY > 255) 	*Y = 255;
	else					*Y = (unsigned char) newY;
}



/**
*
*/
void joystick_reset_calibration()
{
	raw_value[LEFT][BACK][0] = 0;
	raw_value[LEFT][BACK][1] = 0;
	raw_value[LEFT][MIDY][0] = 0;
	raw_value[LEFT][MIDY][1] = 128;
	raw_value[LEFT][FORWARD][0] = 0;
	raw_value[LEFT][FORWARD][1] = 255;
	
	raw_value[MIDX][BACK][0] = 128;
	raw_value[MIDX][BACK][1] = 0;
	raw_value[MIDX][MIDY][0] = 128;
	raw_value[MIDX][MIDY][1] = 128;
	raw_value[MIDX][FORWARD][0] = 128;
	raw_value[MIDX][FORWARD][1] = 255;
	
	raw_value[RIGHT][BACK][0] = 255;
	raw_value[RIGHT][BACK][1] = 0;
	raw_value[RIGHT][MIDY][0] = 255;
	raw_value[RIGHT][MIDY][1] = 128;
	raw_value[RIGHT][FORWARD][0] = 255;
	raw_value[RIGHT][FORWARD][1] = 255;

	min_half_range[0] = 128;
	min_half_range[1] = 128;

	correction_factors_set = 0;
}


/**
*
*/
static void set_correction_factors(void)
{
	int i, result;	

	// Calculate correction factors
	leftY_corr[0] = raw_value[LEFT][MIDY][0] - raw_value[MIDX][MIDY][0]; 
	leftY_corr[1] = raw_value[LEFT][MIDY][1] - raw_value[MIDX][MIDY][1];

	rightY_corr[0] = raw_value[RIGHT][MIDY][0] - raw_value[MIDX][MIDY][0]; 
	rightY_corr[1] = raw_value[RIGHT][MIDY][1] - raw_value[MIDX][MIDY][1];

	backX_corr[0] = raw_value[MIDX][BACK][0] - raw_value[MIDX][MIDY][0]; 
	backX_corr[1] = raw_value[MIDX][BACK][1] - raw_value[MIDX][MIDY][1];

	forwardX_corr[0] = raw_value[MIDX][FORWARD][0] - raw_value[MIDX][MIDY][0]; 
	forwardX_corr[1] = raw_value[MIDX][FORWARD][1] - raw_value[MIDX][MIDY][1];


	min_half_range[0] = 128;
	min_half_range[1] = 128;

	for(i = LEFT; i <= RIGHT; ++i)
	{
		result = abs(raw_value[i][FORWARD][1] - raw_value[i][MIDY][1]);
		if (result < min_half_range[0]) 
			min_half_range[0] = result;

		result = abs(raw_value[i][BACK][1] - raw_value[i][MIDY][1]);
		if (result < min_half_range[0]) 
			min_half_range[0] = result;
	}

	for(i = BACK; i <= FORWARD; ++i)
	{
		result = abs(raw_value[LEFT][i][0] - raw_value[MIDX][i][0]);
		if (result < min_half_range[1]) 
			min_half_range[1] = result;

		result = abs(raw_value[RIGHT][i][0] - raw_value[MIDX][i][0]);
		if (result < min_half_range[1]) 
			min_half_range[1] = result;		 
	} 

	correction_factors_set = 1;
}
